package trb.complexshield.bool;

import javax.media.j3d.Transform3D;
import javax.vecmath.Vector3f;
import no.hrp.hvrc.rad.calc.intersect.Segment;
import no.hrp.hvrc.utils.j3d.BBox;
import no.hrp.hvrc.utils.j3d.Mat4;
import no.hrp.hvrc.utils.j3d.Vec3;

public class IntersectableSphere implements SegmentIntersectable {
    private static final float EPSILON = 0.0001f;

    private float r;

    // the shields local to world matrix
    private final Transform3D local2World = new Transform3D();

    // the world to local space transformation matrix
    protected Transform3D world2local = new Transform3D();

    // temp var, the transformed line
    private static final ThreadLocal<Segment> lThreadLocal = new ThreadLocal<Segment>() {
        @Override
        protected Segment initialValue() {
            return new Segment(new Vector3f(), new Vector3f(0, 1, 0));
        }
    };

    private BBox aabb;

    /**
     * Creates a sphere with the specified radius
     * @param r the radius
     * @param t3d the local2world transform
     */
    public IntersectableSphere(float r, Transform3D t3d) {
        this.r = r;
        world2local.invert(t3d);
        local2World.set(t3d);
        aabb = new BBox(new Vec3(-r, -r, -r), new Vec3(r, r, r));
        aabb.transform(local2World);
    }

    public float getRadius() {
        return r;
    }

    public Mat4 getLocalToWorld() {
        return new Mat4(local2World);
    }

    public Mat4 getWorldToLocal() {
        return new Mat4(world2local);
    }

    @Override
    public boolean intersects(Segment line, float[] tMinMaxOut) {
        Segment l = lThreadLocal.get();
        l.transform(line, world2local);

        Vec3 m = new Vec3(l.originRef());
        float b = m.dot(line.directionRef());
        float c = m.dot(m) - r * r;
        if (c > 0 && b > 0) {
            return false;
        }

        float discr = b*b - c;
        if (discr < 0) {
            return false;
        }

        float sqrt = (float) Math.sqrt(discr);
        float t0 = -b - sqrt;
        float t1 = -b + sqrt;
        t0 /= l.length();
        t1 /= l.length();
        if (t0 > 1) {
            return false;
        }
        if (t0 < 0) {
            t0 = 0;
        }
        if (t1 > 1) {
            t1 = 1;
        }
        tMinMaxOut[0] = t0;
        tMinMaxOut[1] = t1;
        return true;
    }

    public BBox getAABB() {
        return aabb;
    }
}
